// Created by zengyuhao on 25-8-26.
// Copyright (c) 2025 Engineai All rights reserved.
//
#include "robot_manager.h"

void RobotManager::control(const double &t)
{
    Eigen::VectorXd q, qd, qdd, torque;
    robot_algo_->rp_->getJointPoint(t, q, qd, qdd);
    
    // 确保 qd 和 qdd 向量具有正确的维度
    int dof = robot_algo_->dof_;
    if (q.size() == dof)
    {
        // 如果 qd 的维度不正确，则用零向量填充
        if (qd.size() != dof)
        {
            qd.resize(dof);
            qd.setZero();
        }
        
        // 如果 qdd 的维度不正确，则用零向量填充
        if (qdd.size() != dof)
        {
            qdd.resize(dof);
            qdd.setZero();
        }
    }
    else
    {
        ZLOGE("Invalid joint position vector size! Expected: %d, Got: %ld", dof, q.size());
        return;
    }
    
    robot_algo_->id_->inverseDynamics(q, qd, qdd, torque);
    ZLOGI("time [%f]: %f, %f, %f, %f, %f, %f, %f", t, q[0], q[1], q[2], q[3], q[4], q[5], q[6]);
    for (size_t i = 0; i < 7; i++)
    {
        if (!is_simulation_)
        {
            //! 实机模式下还需调用 DM SDK 获取实际的关节角度
            dm_->control_mit(motors_[i], mit_kp_[i], mit_kd_[i], q[i], qd[i], torque[i] * torque_gain_[i]);
        }
        else
        {
            joint_state_msg_.position[i] = q[i];
            //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
            joint_state_msg_.header.stamp = this->now();
            joint_pub_->publish(joint_state_msg_); // 发布初始的 joint_states 消息
        }
    }
}
